#include "app.h"
#include "sht3x_dis.h"
#include "sensor_service.h"
#include "stm32l0xx_nucleo.h"
#include "math.h"
#include "lis2dh12.h"
#include "stm32l052_lp_mode.h"
#include "stm32_bluenrg_ble.h"
#include "x_nucleo_iks01a1.h"
#include "flash.h"

/*private variable************************************************************************/
extern float sensitivity;
static uint32_t viberate_value[3] = {0, 0, 0};
static axis_average_value_t tilt_average;
static uint8_t wake_up_flag = 0;
static axis_type_t axis_buf[AXIS_BUFFER_SIZE] = {0};
/*private function************************************************************************/
static APP_Status_t app_tilt_angle_arithmatic(axis_average_value_t* data);
static APP_Status_t app_viberate_arithmatic(uint32_t* data);
static int32_t app_int_sqrt(uint32_t data);
#ifdef APP_VIBERATE_ABS
static APP_Status_t app_viberate_abs(uint32_t* data);
#endif

static void app_bit_shift(uint8_t* des_data, uint32_t* src_data, uint8_t byte_num)
{
    uint8_t i;
    
    for(i = 0; i < byte_num; i++)
    {
        des_data[i] = (*src_data >> (8 * (byte_num - i -1))) & 0xff;
    }
}

void app_excute_one_time_in_standby_mode(void)
{
#ifdef BlUE_NRG
    bluenrg_start_work();
#endif
}

void app_wakeup_flag_set(uint8_t data)
{
    wake_up_flag = data;
}

uint8_t app_wakeup_flag_get(void)
{
    return wake_up_flag;
}

void app_3axis_int_callback(void)
{
    app_wakeup_flag_set(APP_WAKEUP_STATES_3AXIS);
}

void app_wakeup_handle(void)
{
    uint8_t low_power_wake_up_flag = 0;
    
    low_power_wake_up_flag = app_wakeup_flag_get();
    if(low_power_wake_up_flag == APP_WAKEUP_STATES_3AXIS){
#ifdef APP_TILT_ANGLE
        app_update_adv_tilt_angle();
        HAL_Delay(20);
        bluenrg_stop_adv();
#endif
#ifdef APP_VIBERATE
        app_update_adv_viberate_value();
        HAL_Delay(20);
        bluenrg_stop_adv();
#endif
#ifdef STANDBY_MODE 
       lp_set_wakeup_time(2000);
       lp_standby_mode();
#endif
    }
    
    if(low_power_wake_up_flag == APP_WAKEUP_STATES_TEMP_HUM){
       app_update_adv_tmp_hum_value();
       HAL_Delay(20);
       bluenrg_stop_adv();
       lp_set_wakeup_time(100);
#ifdef STANDBY_MODE 
       lp_set_wakeup_time(2000);
       lp_standby_mode();
#endif
    }
}

void app_update_adv_tmp_hum_value(void)
{
    uint16_t temp = 0, hum = 0;
    uint8_t buf[10] = {10};
    
    adv_manufactory_data_t* manuf_data = (void*)buf;
    manuf_data->len = 9;
    manuf_data->adv_type = AD_TYPE_MANUFACTURER_SPECIFIC_DATA;
    manuf_data->company_id[0] = 0xff;
    manuf_data->company_id[1] = 0xff;
    manuf_data->data_type[0] = (APP_ADV_SENSOR_TEMP_HUMI_TYPE >> 8) & 0xff;
    manuf_data->data_type[1] = APP_ADV_SENSOR_TEMP_HUMI_TYPE & 0xff;
    
    /*get temp, humi value*/
    sht3x_dis_read_single_slot_command(&temp, &hum);
    manuf_data->data[0] = temp >> 8;
    manuf_data->data[1] = temp & 0xff;                    
    manuf_data->data[2] = hum >> 8;
    manuf_data->data[3] = hum & 0xff;                    
                            
    if(bluenrg_adv_manufacture_data((uint8_t*)manuf_data, 10) != 0x00)
         while(1);
}

void app_update_adv_viberate_value(void)
{
    uint8_t buf[18] = {0};
    
    adv_manufactory_data_t* manuf_data = (void*)buf;
    manuf_data->len = 17;
    manuf_data->adv_type = AD_TYPE_MANUFACTURER_SPECIFIC_DATA;
    manuf_data->company_id[0] = 0xff;
    manuf_data->company_id[1] = 0xff;
    manuf_data->data_type[0] = (APP_ADV_SENSOR_VIBERATE_TYPE >> 8) & 0xff;
    manuf_data->data_type[1] = APP_ADV_SENSOR_VIBERATE_TYPE & 0xff;
                             
    /*get vibereate value*/
    app_bit_shift(&manuf_data->data[0], &viberate_value[0], 4);                          
    app_bit_shift(&manuf_data->data[4], &viberate_value[1], 4);                          
    app_bit_shift(&manuf_data->data[4+4], &viberate_value[2], 4);                                                    
                            
    if(bluenrg_adv_manufacture_data((uint8_t*)manuf_data, 18) != 0x00)
         while(1);
}

void app_update_adv_tilt_angle(void)
{
    uint8_t buf[18] = {0};
    
    adv_manufactory_data_t* manuf_data = (void*)buf;
    manuf_data->len = 17;
    manuf_data->adv_type = AD_TYPE_MANUFACTURER_SPECIFIC_DATA;
    manuf_data->company_id[0] = 0xff;
    manuf_data->company_id[1] = 0xff;
    manuf_data->data_type[0] = (APP_ADV_SENSOR_TILT_ANGLE_TYPE >> 8) & 0xff;
    manuf_data->data_type[1] = APP_ADV_SENSOR_TILT_ANGLE_TYPE & 0xff;
    /*get vibereate value*/
    app_bit_shift(&manuf_data->data[0], (uint32_t*)&tilt_average.average_x, 4);                          
    app_bit_shift(&manuf_data->data[4], (uint32_t*)&tilt_average.average_y, 4);                          
    app_bit_shift(&manuf_data->data[4+4], (uint32_t*)&tilt_average.average_z, 4);                                                    
      
   if(bluenrg_adv_manufacture_data((uint8_t*)manuf_data, 18) != 0x00)
         while(1);
}

void app_viberate_value_output(void)
{        
    app_viberate_arithmatic(viberate_value);
}

void app_tilt_angle_output(void)
{
    app_tilt_angle_arithmatic(&tilt_average);
}

static APP_Status_t app_tilt_angle_arithmatic(axis_average_value_t* data)
{
    int16_t i;
    int32_t sum_x = 0, sum_y = 0, sum_z = 0;
 
    /*sum*/
    for(i = 0; i < AXIS_BUFFER_SIZE; i++){
        sum_x += axis_buf[i].axis_x;
        sum_y += axis_buf[i].axis_y;
        sum_z += axis_buf[i].axis_z;
        
    }
   /*average*/
   data->average_x = sum_x / AXIS_BUFFER_SIZE;
   data->average_y = sum_y / AXIS_BUFFER_SIZE;
   data->average_z = sum_z / AXIS_BUFFER_SIZE;
    
   return app_ok;
 
}

static APP_Status_t app_viberate_arithmatic(uint32_t* data)
{
    int16_t i;
    int32_t average_x = 0, average_y = 0, average_z = 0;
    int32_t sum_x = 0, sum_y = 0, sum_z = 0;
    uint32_t sum_sq_x = 0, sum_sq_y = 0, sum_sq_z = 0, average_sq_x = 0, average_sq_y = 0, average_sq_z = 0;
    
    /*sum*/
    for(i = 0; i < AXIS_BUFFER_SIZE; i++){
        sum_x += axis_buf[i].axis_x;
        sum_y += axis_buf[i].axis_y;
        sum_z += axis_buf[i].axis_z;
        
    }
   /*average*/
   average_x = sum_x / AXIS_BUFFER_SIZE;
   average_y = sum_y / AXIS_BUFFER_SIZE;
   average_z = sum_z / AXIS_BUFFER_SIZE;
   /*sum of square*/
   for(i = 0; i < AXIS_BUFFER_SIZE; i++){
       sum_sq_x += ((int32_t)axis_buf[i].axis_x-average_x) * ((int32_t)axis_buf[i].axis_x-average_x);
       sum_sq_y += ((int32_t)axis_buf[i].axis_y-average_y) * ((int32_t)axis_buf[i].axis_y-average_y);
       sum_sq_z += ((int32_t)axis_buf[i].axis_z-average_z) * ((int32_t)axis_buf[i].axis_z-average_z);
   }
   /*average*/
   average_sq_x = sum_sq_x / AXIS_BUFFER_SIZE;
   average_sq_y = sum_sq_y / AXIS_BUFFER_SIZE;
   average_sq_z = sum_sq_z / AXIS_BUFFER_SIZE;
   /*sqrt*/
   data[0] = app_int_sqrt(average_sq_x);
   data[1] = app_int_sqrt(average_sq_y) ;
   data[2] = app_int_sqrt(average_sq_z);
   
   return app_ok;
   
}

#ifdef APP_VIBERATE_ABS
static APP_Status_t app_viberate_abs(uint32_t* data)
{
    int16_t i;
    int32_t average_x = 0, average_y = 0, average_z = 0;
    int32_t sum_x = 0, sum_y = 0, sum_z = 0;
    uint32_t sum_abs_x = 0, sum_abs_y = 0, sum_abs_z = 0;
    
    /*sum*/
    for(i = 0; i < AXIS_BUFFER_SIZE; i++){
        sum_x += axis_buf[i].axis_x;
        sum_y += axis_buf[i].axis_y;
        sum_z += axis_buf[i].axis_z;
        
    }
   /*average*/
   average_x = sum_x / AXIS_BUFFER_SIZE;
   average_y = sum_y / AXIS_BUFFER_SIZE;
   average_z = sum_z / AXIS_BUFFER_SIZE;
   /*sum of square*/
   for(i = 0; i < AXIS_BUFFER_SIZE; i++){
       sum_abs_x += abs((int32_t)axis_buf[i].axis_x-average_x);
       sum_abs_y += abs((int32_t)axis_buf[i].axis_y-average_y);
       sum_abs_z += abs((int32_t)axis_buf[i].axis_z-average_z);
   }
   /*average*/
   data[0] = sum_abs_x / AXIS_BUFFER_SIZE;
   data[1] = sum_abs_y / AXIS_BUFFER_SIZE;
   data[2] = sum_abs_z / AXIS_BUFFER_SIZE;
   
   return app_ok;
}
#endif

static int32_t app_int_sqrt(uint32_t data)
{
    int32_t r = 0, t = 0;
    int8_t i = 0;
    
    for(i = 15; i >= 0; i--){
        t = r | (int32_t)( 0x01 << i);
        if(((t) * (t)) < data){
            r = t;
        }
    }
    
    return r;
}


/**
 * @brief  EXTI line detection callback.
 * @param  Specifies the pins connected EXTI line
 * @retval None
 */

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    static uint8_t count;
    static int16_t axis_flag = 0;
 
    switch (GPIO_Pin){
     case BNRG_SPI_IRQ_PIN:
         HCI_Isr();
     break;
     case SHT3X_DIS_ALERT_INT_PIN:
         
     break;
     case LIS2DH12_INT2_PIN:

     break;
     case LIS2DH12_INT1_PIN:
        if(axis_flag < AXIS_BUFFER_SIZE){
             for(count = 0; count < LIS2DH12_SAMPLE_NUMBER; count++){
                 if(LIS2DH12_X_Y_Z_Get_Axes_Raw((lis2dh12_axis_type_t*)&axis_buf[count + axis_flag]) != 0x00)
                 return;
                 
             }

             axis_flag += LIS2DH12_SAMPLE_NUMBER;
        }
        
        if(axis_flag == AXIS_BUFFER_SIZE){
            
#ifdef APP_TILT_ANGLE
            app_tilt_angle_output();
#endif
#ifdef APP_VIBERATE
            app_viberate_value_output();
#endif
            axis_flag = 0;
            app_3axis_int_callback();
        } 
     break;
     default:
     break;
    }
}

/*flash***************************************************************************************/
void app_led_toggle_times(uint8_t times)
{
    uint8_t i;
    for(i = 0; i < times; i++)
    {
        BSP_LED_Toggle(LED5);
        HAL_Delay(500);
        BSP_LED_Toggle(LED5);
        HAL_Delay(500);
    }

}

void app_flash_write_check(void)
{
    uint32_t ret = 0;
    uint32_t tmp_buf[4] = {0x12345678, 0x9abcdef1, 0x87654321, 0x1fedcba9};
    
    ret = flash_page_write(FLASH_USER_START_ADDR, tmp_buf, 1);
    if(ret != app_ok){
        app_led_toggle_times(2);
        while(1);
    }
    
    ret = flash_page_content_check(FLASH_USER_START_ADDR, tmp_buf, 1);
    if(ret != app_ok){
        app_led_toggle_times(4);
        while(1);
    }
    
    app_led_toggle_times(6);
    
}


